class QMI8658Reg:
    QMI8658_WHO_AM_I = 0x00
    QMI8658_REVISION_ID = 0x01
    QMI8658_CTRL1 = 0x02
    QMI8658_CTRL2 = 0x03
    QMI8658_CTRL3 = 0x04
    QMI8658_CTRL4 = 0x05
    QMI8658_CTRL5 = 0x06
    QMI8658_CTRL6 = 0x07
    QMI8658_CTRL7 = 0x08
    QMI8658_CTRL8 = 0x09
    QMI8658_CTRL9 = 0x0A
    QMI8658_CATL1_L = 0x0B
    QMI8658_CATL1_H = 0x0C
    QMI8658_CATL2_L = 0x0D
    QMI8658_CATL2_H = 0x0E
    QMI8658_CATL3_L = 0x0F
    QMI8658_CATL3_H = 0x10
    QMI8658_CATL4_L = 0x11
    QMI8658_CATL4_H = 0x12
    QMI8658_FIFO_WTM_TH = 0x16
    QMI8658_FIFO_CTRL = 0x17
    QMI8658_FIFO_SMPL_CNT = 0x18
    QMI8658_FIFO_STATUS = 0x19
    QMI8658_FIFO_DATA = 0x1A
    QMI8658_STATUSINT = 0x2D
    QMI8658_STATUS0 = 0x2E
    QMI8658_STATUS1 = 0x2F
    QMI8658_TIMESTAMP_LOW = 0x30
    QMI8658_TIMESTAMP_MID = 0x31
    QMI8658_TIMESTAMP_HIGH = 0x32
    QMI8658_TEMP_L = 0x33
    QMI8658_TEMP_H = 0x34
    QMI8658_AX_L = 0x35
    QMI8658_AX_H = 0x36
    QMI8658_AY_L = 0x37
    QMI8658_AY_H = 0x38
    QMI8658_AZ_L = 0x39
    QMI8658_AZ_H = 0x3A
    QMI8658_GX_L = 0x3B
    QMI8658_GX_H = 0x3C
    QMI8658_GY_L = 0x3D
    QMI8658_GY_H = 0x3E
    QMI8658_GZ_L = 0x3F
    QMI8658_GZ_H = 0x40
    QMI8658_COD_STATUS = 0x46
    QMI8658_dQW_L = 0x49
    QMI8658_dQW_H = 0x4A
    QMI8658_dQX_L = 0x4B
    QMI8658_dQX_H = 0x4C
    QMI8658_dQY_L = 0x4D
    QMI8658_dQY_H = 0x4E
    QMI8658_dQZ_L = 0x4F
    QMI8658_dQZ_H = 0x50
    QMI8658_dVX_L = 0x51
    QMI8658_dVX_H = 0x52
    QMI8658_dVY_L = 0x53
    QMI8658_dVY_H = 0x54
    QMI8658_dVZ_L = 0x55
    QMI8658_dVZ_H = 0x56
    QMI8658_TAP_STATUS = 0x59
    QMI8658_STEP_CNT_LOW = 0x5A
    QMI8658_STEP_CNT_MIDL = 0x5B
    QMI8658_STEP_CNT_HIGH = 0x5C
    QMI8658_RESET = 0x60
class QMI8658(object):
    def __init__(self,i2c,address=0x6A):
        self._address = address
        self._bus = i2c
        bRet = self.WhoAmI()
        if bRet:
            self.Read_Revision()
        else:
            return NULL
        self.Config_apply()
        # 配置运动状态检测
        self._write_byte(QMI8658Reg.QMI8658_CATL1_L, 1)    # AnyMotionXThr
        self._write_byte(QMI8658Reg.QMI8658_CATL1_H, 1)    # AnyMotionYThr
        self._write_byte(QMI8658Reg.QMI8658_CATL2_L, 1)    # AnyMotionZThr
        self._write_byte(QMI8658Reg.QMI8658_CATL2_H, 1)    # NoMotionXThr
        self._write_byte(QMI8658Reg.QMI8658_CATL3_L, 1)    # NoMotionYThr
        self._write_byte(QMI8658Reg.QMI8658_CATL3_H, 1)    # NoMotionZThr
        self._write_byte(QMI8658Reg.QMI8658_CATL4_L, 0x77) # MOTION_MODE_CTRL 0111 0111
        self._write_byte(QMI8658Reg.QMI8658_CATL4_H, 0x01) # 0x01(means 1st command)
        self._write_byte(QMI8658Reg.QMI8658_CTRL9, 0x0E)   # CTRL_CMD_CONFIGURE_MOTION

        self._write_byte(QMI8658Reg.QMI8658_CATL1_L, 1)    # AnyMotionWindow
        self._write_byte(QMI8658Reg.QMI8658_CATL1_H, 1)    # NoMotionWindow
        self._write_byte(QMI8658Reg.QMI8658_CATL2_L, 0xE8) # SigMotionWaitWindow[7:0]
        self._write_byte(QMI8658Reg.QMI8658_CATL2_H, 0x03) # SigMotionWaitWindow [15:8]
        self._write_byte(QMI8658Reg.QMI8658_CATL3_L, 0xE8) # SigMotionConfirmWindow[7:0]
        self._write_byte(QMI8658Reg.QMI8658_CATL3_H, 0x03) # SigMotionConfirmWindow[15:8]
        self._write_byte(QMI8658Reg.QMI8658_CATL4_H, 0x02) # 0x02(means 2nd command)
        self._write_byte(QMI8658Reg.QMI8658_CTRL9, 0x0E)   # CTRL_CMD_CONFIGURE_MOTION

        self._write_byte(QMI8658Reg.QMI8658_CTRL1, 0x40)   # CTRL1 设置地址自动增加
        self._write_byte(QMI8658Reg.QMI8658_CTRL7, 0x03)   # CTRL7 允许加速度和陀螺仪
        self._write_byte(QMI8658Reg.QMI8658_CTRL2, 0x95)   # CTRL2 设置ACC 4g 250Hz
        self._write_byte(QMI8658Reg.QMI8658_CTRL3, 0xd5)   # CTRL3 设置GRY 512dps 250Hz

        self._write_byte(QMI8658Reg.QMI8658_CTRL8, 0x0E)
    def _read_byte(self, cmd):
        rec = self._bus.readfrom_mem(int(self._address), int(cmd), 1)
        return rec[0]

    def _read_block(self, reg, length=1):
        rec = self._bus.readfrom_mem(int(self._address), int(reg), length)
        return rec

    def _read_u16(self, cmd):
        LSB = self._bus.readfrom_mem(int(self._address), int(cmd), 1)
        MSB = self._bus.readfrom_mem(int(self._address), int(cmd) + 1, 1)
        return (MSB[0] << 8) + LSB[0]

    def _write_byte(self, cmd, val):
        self._bus.writeto_mem(int(self._address), int(cmd), bytes([int(val)]))

    def WhoAmI(self):
        bRet = False
        if (0x05) == self._read_byte(0x00):
            bRet = True
        return bRet

    def Read_Revision(self):
        return self._read_byte(0x01)

    def Config_apply(self):
        # REG CTRL1
        self._write_byte(0x02, 0x60)
        # REG CTRL2 : QMI8658AccRange_8g  and QMI8658AccOdr_1000Hz
        self._write_byte(0x03, 0x20)
        # REG CTRL3 : QMI8658GyrRange_512dps and QMI8658GyrOdr_1000Hz
        self._write_byte(0x04, 0x50)
        # REG CTRL4 : No
        self._write_byte(0x05, 0x00)
        # REG CTRL5 : Enable Gyroscope And Accelerometer Low-Pass Filter
        self._write_byte(0x06, 0x11)
        # REG CTRL6 : AttitudeEngine ODR. 64hz MoD off
        self._write_byte(0x07, 0x06)
        # REG CTRL7 : Enable Gyroscope And Accelerometer/AttitudeEngine
        self._write_byte(0x08, 0x4B)

    def Read_Raw_Quat(self):
        #print(" ")
        wxyz = [1, 0, 0, 0]
        self._write_byte(0x0A, 0x0C)
        raw_timestamp = self._read_block(0x30, 3)
        quat = self._read_block(0x45, 8)
        #print("quat: ")
        #print(quat)
        status = self._read_block(0x2F, 1)
        #print("status: ")
        #print(status)
        timestamp = (
            (raw_timestamp[2] << 16) | (raw_timestamp[1] << 8) | (raw_timestamp[0])
        )
        #print("Timestamp: ")
        #print(timestamp)
        #temp = self._read_byte(0x33)
        #print("temp")
        #print(temp)
        for i in range(3):
            wxyz[i] = (wxyz[(i) + 1]) | (wxyz[i])
        #print(wxyz)
        return wxyz

    def Read_Raw_XYZ(self):
        xyz = [0, 0, 0, 0, 0, 0]
        raw_timestamp = self._read_block(0x30, 3)
        raw_acc_xyz = self._read_block(0x35, 6)
        raw_gyro_xyz = self._read_block(0x3B, 6)
        raw_xyz = self._read_block(0x35, 12)
        timestamp = (
            (raw_timestamp[2] << 16) | (raw_timestamp[1] << 8) | (raw_timestamp[0])
        )
        for i in range(6):
            # xyz[i]=(raw_acc_xyz[(i*2)+1]<<8)|(raw_acc_xyz[i*2])
            # xyz[i+3]=(raw_gyro_xyz[((i+3)*2)+1]<<8)|(raw_gyro_xyz[(i+3)*2])
            xyz[i] = (raw_xyz[(i * 2) + 1] << 8) | (raw_xyz[i * 2])
            if xyz[i] >= 32767:
                xyz[i] = xyz[i] - 65535
        return xyz

    def Read_XYZ(self):
        xyz = [0, 0, 0, 0, 0, 0]
        raw_xyz = self.Read_Raw_XYZ()
        # QMI8658AccRange_8g
        acc_lsb_div = 1 << 12
        # QMI8658GyrRange_512dps
        gyro_lsb_div = 64
        for i in range(3):
            xyz[i] = raw_xyz[i] / acc_lsb_div  # (acc_lsb_div/1000.0)
            xyz[i + 3] = raw_xyz[i + 3] * 1.0 / gyro_lsb_div
        return xyz
    def Fetch_motion(self):
        """
        获取Motion状态
        """
        status = self._read_byte(0x2f) 
        return status
